using System.Collections.Generic;
using Unity.MLAgents;
using UnityEngine;
public enum Team
{
    Red = 0,
    Blue = 1
}

public class SatelliteEnvController : MonoBehaviour
{
    [System.Serializable]
    public class PursuerInfo
    {
        public AgentSatelliteP Agent;
        [HideInInspector]
        public Vector3 StartingPos;
        [HideInInspector]
        public Quaternion StartingRot;
        [HideInInspector]
        public Rigidbody Rb;
    }

    [System.Serializable]
    public class EvaderInfo
    {
        public AgentSatelliteE Agent;
        [HideInInspector]
        public Vector3 StartingPos;
        [HideInInspector]
        public Quaternion StartingRot;
        [HideInInspector]
        public Rigidbody Rb;
    }

    /// <summary>
    /// Max Academy steps before this platform resets
    /// </summary>
    /// <returns></returns>
 
    //0.02s   10000*0.02=200s

    [Tooltip("Max Environment Steps")] public int MaxEnvironmentSteps = 12000;

    //List of Agents On Platform
    public List<PursuerInfo> PursuerList = new List<PursuerInfo>();
    public List<EvaderInfo> EvaderList = new List<EvaderInfo>();

    //private SoccerSettings m_SoccerSettings;

    private SimpleMultiAgentGroup m_RedPursuerGroup;
    private SimpleMultiAgentGroup m_BlueEvaderGroup;

    private int m_ResetTimer;




    void Start()
    {

      //  m_SoccerSettings = FindObjectOfType<SoccerSettings>();
        // Initialize TeamManager
        m_RedPursuerGroup = new SimpleMultiAgentGroup();
        m_BlueEvaderGroup = new SimpleMultiAgentGroup();
       
        foreach (var item in PursuerList)
        {
            item.StartingPos = item.Agent.transform.localPosition;
            item.StartingRot = item.Agent.transform.localRotation;
            item.Rb = item.Agent.GetComponent<Rigidbody>();
            m_RedPursuerGroup.RegisterAgent(item.Agent);
        } 
        foreach (var item in EvaderList)
        {
            item.StartingPos = item.Agent.transform.localPosition;
            item.StartingRot = item.Agent.transform.localRotation;
            item.Rb = item.Agent.GetComponent<Rigidbody>();
            m_BlueEvaderGroup.RegisterAgent(item.Agent);
        } 

        ResetScene();
    }
      
    void FixedUpdate()
    {
        
        m_ResetTimer += 1;
        
        var agentred1 = PursuerList[0];
        
        
        var agentred2 = PursuerList[1];
        var agentred3 = PursuerList[2];
        var agentred4 = PursuerList[3]; 

        var agentblue = EvaderList[0];
        // 过程奖励和终端奖励
        // 过程奖励   距离 速度夹角？
        Vector3 posagentred1 = agentred1.Agent.transform.localPosition;
        Vector3 posagentblue = agentblue.Agent.transform.localPosition;
        Vector3 relaPos1 = posagentblue-posagentred1;

        Vector3 posagentred2 = agentred2.Agent.transform.localPosition;
        Vector3 relaPos2 = posagentblue - posagentred2;

        Vector3 posagentred3 = agentred3.Agent.transform.localPosition;
        Vector3 relaPos3 = posagentblue - posagentred3;

        Vector3 posagentred4 = agentred4.Agent.transform.localPosition;
        Vector3 relaPos4 = posagentblue - posagentred4;

        Vector3 velagentred1 = agentred1.Rb.velocity;
        Vector3 velagentred2 = agentred2.Rb.velocity;
        Vector3 velagentred3 = agentred3.Rb.velocity;
        Vector3 velagentred4 = agentred4.Rb.velocity;
        Vector3 velagentblue = agentblue.Rb.velocity;
        
        //persuer 与 evader 之间的距离
        float distancePE1 = relaPos1.magnitude;
        float distancePE2 = relaPos2.magnitude;
        float distancePE3 = relaPos3.magnitude;
        float distancePE4 = relaPos4.magnitude;

        // persuer 之间的距离
        Vector3 relaPos12 = posagentred1 - posagentred2;
        Vector3 relaPos13 = posagentred1 - posagentred3;
        Vector3 relaPos14 = posagentred1 - posagentred4;
        Vector3 relaPos23 = posagentred2 - posagentred3;
        Vector3 relaPos24 = posagentred2 - posagentred4;
        Vector3 relaPos34 = posagentred3 - posagentred4;

        float distanceP12 = relaPos12.magnitude;
        float distanceP13 = relaPos13.magnitude;
        float distanceP14 = relaPos14.magnitude;
        float distanceP23 = relaPos23.magnitude;
        float distanceP24 = relaPos24.magnitude;
        float distanceP34 = relaPos34.magnitude;


        //速度是它自己的速度，可别再相对速度了
        float thetaP1 = Vector3.Dot(relaPos1.normalized, velagentred1.normalized);
        float thetaP2 = Vector3.Dot(relaPos2.normalized, velagentred2.normalized);
        float thetaP3 = Vector3.Dot(relaPos3.normalized, velagentred3.normalized);
        float thetaP4 = Vector3.Dot(relaPos4.normalized, velagentred4.normalized);

        float thetaE1 = Vector3.Dot(relaPos1.normalized, velagentblue.normalized);  // 矢量由红方指向蓝方
        float thetaE2 = Vector3.Dot(relaPos2.normalized, velagentblue.normalized);
        float thetaE3 = Vector3.Dot(relaPos3.normalized, velagentblue.normalized);
        float thetaE4 = Vector3.Dot(relaPos4.normalized, velagentblue.normalized);
        //m_RedPursuerGroup.AddGroupReward(s2peed*0.01f);
        //Debug.Log("relapos:"+relaPos.norma3lized+"relavel:"+relavel.normalized);
        //Debug.Log("点积:"+speed);         4
        //Debug.Log("velagentblue:"+velagentblue+"posagentblue:"+posagentblue);


        int maxD=500; // 假设1000为最大可能距离
        // 从向量平行到向量共线，平行也可以了
        m_RedPursuerGroup.AddGroupReward(( - distancePE1 / maxD) * 0.001f);//1-distancePE/maxD  theta*0.001f   thetaP1
        m_RedPursuerGroup.AddGroupReward(( - distancePE2 / maxD) * 0.001f);//1-distancePE/maxD  theta*0.001f   thetaP2
        m_RedPursuerGroup.AddGroupReward(( - distancePE3 / maxD) * 0.001f);//1-distancePE/maxD  theta*0.001f    thetaP3
        m_RedPursuerGroup.AddGroupReward(( - distancePE4 / maxD) * 0.001f);//1-distancePE/maxD  theta*0.001f     thetaP4
        //m_BlueEvaderGroup.AddGroupReward((thetaE1 + thetaE2 + thetaE3 + thetaE4) * 0.001f);//distancePE/maxD
        //Debug.Log(1-distancePE/maxD);
        //Debug.Log("evaderReward:"+distancePE/maxD);
        
         
        double capD = 15;  // 捕获界限
        // 捕获结束判断
        if (distancePE1 <= capD && distancePE2 <= capD && distancePE3 <= capD && distancePE4 <= capD)    // 不能同时到达这个，别的星到了，另外的星没到，随着时间流失，别的星就飞了
        //if (distancePE1 <= capD )   // 调试 只用一个追  可以追上
        {
            m_RedPursuerGroup.AddGroupReward(0);//1 - (float)m_ResetTimer / MaxEnvironmentSteps
            m_BlueEvaderGroup.AddGroupReward(0);//-1

            m_BlueEvaderGroup.GroupEpisodeInterrupted();
            m_RedPursuerGroup.GroupEpisodeInterrupted();
            ResetScene();
            Debug.Log("1");
        }
        // 未捕获到结束判断
        if (m_ResetTimer >= MaxEnvironmentSteps && MaxEnvironmentSteps > 0)
        {
            m_BlueEvaderGroup.AddGroupReward(0);//1+ distancePE/maxD
            m_RedPursuerGroup.AddGroupReward(0);//-distancePE/maxD
            m_BlueEvaderGroup.GroupEpisodeInterrupted();
            m_RedPursuerGroup.GroupEpisodeInterrupted();
            ResetScene();
            Debug.Log("2");
        }
        // 未捕获到结束判断
        if (distancePE1 > maxD || distancePE2 > maxD || distancePE3 > maxD || distancePE4 > maxD)
        {
            m_BlueEvaderGroup.AddGroupReward(0);//2
            m_RedPursuerGroup.AddGroupReward(0);//-2
            m_BlueEvaderGroup.GroupEpisodeInterrupted();
            m_RedPursuerGroup.GroupEpisodeInterrupted();
            ResetScene();
            Debug.Log("3");
        }
        // 碰撞判断
        
        if(distanceP12 < capD/2 || distanceP13 < capD / 2 || distanceP14 < capD / 2 || distanceP23 < capD / 2 || distanceP24 < capD / 2 || distanceP34 < capD / 2)
        {
            m_BlueEvaderGroup.AddGroupReward(20f);
            m_RedPursuerGroup.AddGroupReward(-20f);
            m_BlueEvaderGroup.GroupEpisodeInterrupted();
            m_RedPursuerGroup.GroupEpisodeInterrupted();
            ResetScene();
            Debug.Log("collision");
        }
        
    }


    public void ResetScene()
    {
        m_ResetTimer = 0;

        // 红方蓝方可以用相同的策略吗，一个是追一个是逃
        //红方初始位置
        foreach (var item in PursuerList)
        {
            var randomPosX = Random.Range(-2f, 2f);
            var randomPosY = Random.Range(-2f, 2f);
            var randomPosZ = Random.Range(-2f, 2f);
            var newStartPos = item.Agent.initialPos + new Vector3(randomPosX, randomPosY, randomPosZ);           
            var newRot = Quaternion.Euler(0, 0, 0);
            item.Agent.transform.SetPositionAndRotation(newStartPos, newRot);
            item.Rb.velocity = Vector3.zero;
            item.Rb.angularVelocity = Vector3.zero;
        }
        foreach (var item in EvaderList)
        {
            var randomPosX = Random.Range(-5f, 5f);
            var randomPosY = Random.Range(-5f, 5f);
            var randomPosZ = Random.Range(-10f, 10f);
            var newStartPos = item.Agent.initialPos;           
            var newRot = Quaternion.Euler(0, 0, 0);
            item.Agent.transform.SetPositionAndRotation(newStartPos, newRot);
            item.Rb.velocity = Vector3.zero;
            item.Rb.angularVelocity = Vector3.zero;
        }

    }
}
